#include "Matrix.h"
#include <math.h>
#include <memory.h>

Matrix::Matrix(int columnLength)
{
    _columnLength = columnLength;
    _size = columnLength*columnLength;
	_matrix = new double[_size];
	loadIdentity();
}

Matrix::Matrix(int columnLength, double* m)
{
	_columnLength = columnLength;
	_size = columnLength*columnLength;
	_matrix = new double[_size];
	for(int i = 0; i < _size; ++i)
		_matrix[i] = m[i];
}

Matrix::Matrix(const Matrix &m)
{
    _columnLength = m.columnLength();
    _size = m.size();
	_matrix = new double[_size];
	for(int i = 0; i < _size; ++i)
		_matrix[i] = m[i];
}

Matrix::Matrix()
{
	_columnLength = 0;
	_size = 0;
	_matrix = 0;
}


Matrix::~Matrix()
{
	if(_matrix != 0)
		delete[] _matrix;
	_matrix = 0;
}

void Matrix::init()
{
	_matrix = 0; 
	_size = 0; 
	_columnLength = 0;
}

Matrix& Matrix::operator=(Matrix matrix)
{
	_columnLength = matrix.columnLength();
    _size = matrix.size();
	if(_matrix != NULL)
		delete[] _matrix;
	_matrix = new double[_size];
	for(int i = 0; i < _size; ++i)
		_matrix[i] = matrix[i];
	
    return *this;
}

double Matrix::getRowMajorElement(const int i) const
{
    int row = i/_columnLength;
    int col = i%_columnLength;
    return _matrix[col*_columnLength + row];
}

Matrix Matrix::operator+(Matrix &matrix) const
{

	double* m = new double[_size];
	
	for(int i = 0; i < _size; ++i)
		m[i] = _matrix[i] + matrix[i];

	Matrix returnMatrix(_columnLength, m);

	delete[] m;

    return returnMatrix;
}

Matrix Matrix::operator-(Matrix &matrix) const
{

	double* m = new double[_size];

	for(int i = 0; i < _size; ++i)
		m[i] = _matrix[i] - matrix[i];

	Matrix returnMatrix(_columnLength, m);

	delete[] m;

    return returnMatrix;
}

Matrix Matrix::operator*(Matrix &matrix) const
{

	double* m = new double[_size];

	for(int i = 0; i < _size; ++i)
		m[i] = calculateMatrixMultElement(i, _matrix, matrix.getMatrix());

	Matrix returnMatrix(_columnLength, m);

	delete[] m;

    return returnMatrix;
}

double Matrix::calculateMatrixMultElement (int pos, const double* first, const double* second) const
{
    double result = 0;
    int col = pos/_columnLength;
    int row = pos%_columnLength;
    for(int i = 0; i < _columnLength; ++i){
        result += (first[row+i*_columnLength] * second[_columnLength*col+i]);
    }
    return result;

}

Matrix Matrix::operator*( QuantizationMatrix &matrix ) const
{
    double* m = new double[_size];

    for(int i=0; i < _size; ++i)
        m[i] = _matrix[i] * matrix[i];

    Matrix returnMatrix(_columnLength, m);

	delete[] m;

    return returnMatrix;
}

Matrix Matrix::operator / ( QuantizationMatrix &matrix ) const
{
    double* m = new double[_size];

    for(int i=0; i < _size; ++i)
        m[i] = _matrix[i] / matrix[i];

    Matrix returnMatrix(_columnLength, m);

	delete[] m;

    return returnMatrix;
}

void Matrix::loadIdentity()
{
	int k = 0;
	for(int i = 0; i < _columnLength; ++i)
	{
		for(int j =0; j < _columnLength; ++j)
		{
			if(i == j)
				_matrix[k] = 1;
			else
				_matrix[k] = 0;

			++k;
		}
	}
}

void Matrix::addColumn(uint8_t *column, int columnNumber)
{
	for(int i = 0; i < _columnLength; ++i)
	{
		_matrix[columnNumber*_columnLength+i] = column[i];
	}
}

void Matrix::round()
{
    for(int i=0; i < _size; ++i) {

		_matrix[i] += 0.5f;

        _matrix[i] = floor(_matrix[i]);
    }
}

void Matrix::normalize()
{
    for(int i=0; i < _size; ++i)
        _matrix[i] -= 128;
}

void Matrix::deNormalize()
{
    for(int i=0; i < _size; ++i)
    {
        if(_matrix[i] < 0)
            _matrix[i]+=129;
        else
            _matrix[i]+=128;
    }
}

void Matrix::flattenHighValues()
{
    for(int i=0; i < _size; ++i)
    {
        if(_matrix[i] > 127)
            _matrix[i] = 127;
        else if(_matrix[i] < -128)
            _matrix[i] = -128;
    }
}

void Matrix::setMatrix(double* matrix)
{
	_matrix = matrix;
}

void Matrix::setMatrixRowMajor(double *matrix)
{
    for(int i=0; i < _columnLength; ++i)
        for(int j=0; j < _columnLength; ++j)
            _matrix[i*_columnLength+j] = matrix[j*_columnLength+i];
}

void Matrix::setMatrixRowMajor(uint8_t *matrix)
{
    for(int i=0; i < _columnLength; ++i)
        for(int j=0; j < _columnLength; ++j)
            _matrix[i*_columnLength+j] = (double) matrix[j*_columnLength+i];

}

int8_t *Matrix::zigzag()
{
    int8_t *list = new int8_t[_size];

	list[0] = _matrix[0];
    list[1] = _matrix[4];
    list[2] = _matrix[1];
    list[3] = _matrix[2];
    list[4] = _matrix[5];
    list[5] = _matrix[8];
    list[6] = _matrix[12];
    list[7] = _matrix[9];
    list[8] = _matrix[6];
    list[9] = _matrix[3];
    list[10] = _matrix[7];
    list[11] = _matrix[10];
    list[12] = _matrix[13];
    list[13] = _matrix[14];
    list[14] = _matrix[11];
    list[15] = _matrix[15];

//     bool insertEnd = false;
// 
//     int pos = 0;
// 
//     for(int i = 0; i < _columnLength; ++i)
//     {
//         int k = 0;
//         int j = i;
//         while(j >= 0)
//         {
//             int currentPos = j+k*_columnLength;
//             if(!insertEnd)
//                 list[pos] = _matrix[currentPos];
//             else
//                 list[pos+(j-k)] = _matrix[currentPos];
//             --j;
//             ++k;
//             ++pos;
//         }
//         insertEnd = !insertEnd;
//     }
// 
//     for(int i = 1; i < _columnLength; ++i)
//     {
//         int k = i;
//         int j = _columnLength-1;
//         while(k < _columnLength)
//         {
//             if(!insertEnd)
//                 list[pos] = _matrix[j+k*_columnLength];
//             else
//                 list[pos+(j-k)] = _matrix[j+k*_columnLength];
//             --j;
//             ++k;
//             ++pos;
// 
//         }
//         insertEnd = !insertEnd;
//     }


    return list;
}

void Matrix::setElement(int i, double e)
{
    _matrix[i] = e;
}

void Matrix::newMove1(int &c, int &r, int &cMoveMem, int &rMoveMem, int &move)
{
    if( (cMoveMem+c) >= 0 && (rMoveMem+r) >= 0 ) {
        cMoveMem += c;
        rMoveMem += r;
        move += (r+c*_columnLength);
    } else if( (cMoveMem+c) < 0 ) {
        rMoveMem += r;
        move += r;
        c = 1;
        r = -1;
    } else if( (rMoveMem+r) < 0 ) {
        rMoveMem += c;
        move += (c*_columnLength);
        c = -1;
        r = 1;
    }
}

void Matrix::newMove2(int &c, int &r, int &cMoveMem, int &rMoveMem, int &move)
{
    if( (cMoveMem+c) < _columnLength && (rMoveMem+r) < _columnLength ) {
        cMoveMem += c;
        rMoveMem += r;
        move += (r+c*_columnLength);
    } else if( (cMoveMem+c) >= _columnLength ) {
        rMoveMem += r;
        move += r;
        c = -1;
        r = 1;
    } else if( (rMoveMem+r) >= _columnLength ) {
        rMoveMem += c;
        move += (c*_columnLength);
        c = 1;
        r = -1;
    }
}

void Matrix::setSize(int columnLength)
{
    _columnLength = columnLength;
    _size = columnLength*columnLength;
	if(_matrix != NULL)
		delete[] _matrix;
	_matrix = new double[_size];
	loadIdentity();
}

uint8_t* Matrix::getMatrixData()
{
    uint8_t* data = new uint8_t[_size];

    for(int i = 0; i < _size; ++i)
        data[i] = (uint8_t)getRowMajorElement(i);

    return data;
}